// Copyright 2009 Emilie Gillet.
//
// Author: Emilie Gillet (emilie.o.gillet@gmail.com)
//
// This program is free software: you can redistribute it and/or modify
// it under the terms of the GNU General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
// (at your option) any later version.
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
// GNU General Public License for more details.
// You should have received a copy of the GNU General Public License
// along with this program.  If not, see <http://www.gnu.org/licenses/>.

#include "hardware/hal/init_atmega.h"
#include "hardware/hal/serial.h"
#include "hardware/utils/pretty_printer.h"

#include "hardware/resources/resources_manager.h"

using namespace hardware_hal;
using namespace hardware_utils;
using namespace hardware_resources;

// Debug text output.
typedef Serial<SerialPort0, 9600, DISABLED, POLLED> Debug;
PrettyPrinter<Debug> debug_output;

AnalogInput<0> waveform_pot;
AnalogInput<1> amplitude_pot;
AnalogInput<2> frequency_pot;

const prog_uint16_t lut_res_oscillator_increments[] PROGMEM = {
    4389,   4393,   4397,   4401,   4405,   4409,   4413,   4417, 
    4421,   4425,   4429,   4433,   4437,   4441,   4445,   4449, 
    4453,   4457,   4461,   4465,   4469,   4473,   4477,   4481, 
    4485,   4489,   4493,   4497,   4501,   4505,   4509,   4513, 
    4517,   4522,   4526,   4530,   4534,   4538,   4542,   4546, 
    4550,   4554,   4558,   4563,   4567,   4571,   4575,   4579, 
    4583,   4587,   4591,   4596,   4600,   4604,   4608,   4612, 
    4616,   4621,   4625,   4629,   4633,   4637,   4641,   4646, 
    4650,   4654,   4658,   4662,   4667,   4671,   4675,   4679, 
    4684,   4688,   4692,   4696,   4700,   4705,   4709,   4713, 
    4717,   4722,   4726,   4730,   4735,   4739,   4743,   4747, 
    4752,   4756,   4760,   4765,   4769,   4773,   4777,   4782, 
    4786,   4790,   4795,   4799,   4803,   4808,   4812,   4816, 
    4821,   4825,   4830,   4834,   4838,   4843,   4847,   4851, 
    4856,   4860,   4865,   4869,   4873,   4878,   4882,   4887, 
    4891,   4895,   4900,   4904,   4909,   4913,   4917,   4922, 
    4926,   4931,   4935,   4940,   4944,   4949,   4953,   4958, 
    4962,   4967,   4971,   4976,   4980,   4985,   4989,   4994, 
    4998,   5003,   5007,   5012,   5016,   5021,   5025,   5030, 
    5034,   5039,   5043,   5048,   5052,   5057,   5062,   5066, 
    5071,   5075,   5080,   5084,   5089,   5094,   5098,   5103, 
    5107,   5112,   5117,   5121,   5126,   5131,   5135,   5140, 
    5145,   5149,   5154,   5158,   5163,   5168,   5172,   5177, 
    5182,   5186,   5191,   5196,   5201,   5205,   5210,   5215, 
    5219,   5224,   5229,   5233,   5238,   5243,   5248,   5252, 
    5257,   5262,   5267,   5271,   5276,   5281,   5286,   5290, 
    5295,   5300,   5305,   5310,   5314,   5319,   5324,   5329, 
    5334,   5338,   5343,   5348,   5353,   5358,   5363,   5367, 
    5372,   5377,   5382,   5387,   5392,   5397,   5401,   5406, 
    5411,   5416,   5421,   5426,   5431,   5436,   5441,   5446, 
    5450,   5455,   5460,   5465,   5470,   5475,   5480,   5485, 
    5490,   5495,   5500,   5505,   5510,   5515,   5520,   5525, 
    5530,   5535,   5540,   5545,   5550,   5555,   5560,   5565, 
    5570,   5575,   5580,   5585,   5590,   5595,   5600,   5605, 
    5610,   5615,   5620,   5625,   5630,   5636,   5641,   5646, 
    5651,   5656,   5661,   5666,   5671,   5676,   5682,   5687, 
    5692,   5697,   5702,   5707,   5712,   5718,   5723,   5728, 
    5733,   5738,   5743,   5749,   5754,   5759,   5764,   5769, 
    5775,   5780,   5785,   5790,   5795,   5801,   5806,   5811, 
    5816,   5822,   5827,   5832,   5837,   5843,   5848,   5853, 
    5859,   5864,   5869,   5874,   5880,   5885,   5890,   5896, 
    5901,   5906,   5912,   5917,   5922,   5928,   5933,   5938, 
    5944,   5949,   5955,   5960,   5965,   5971,   5976,   5981, 
    5987,   5992,   5998,   6003,   6009,   6014,   6019,   6025, 
    6030,   6036,   6041,   6047,   6052,   6058,   6063,   6068, 
    6074,   6079,   6085,   6090,   6096,   6101,   6107,   6112, 
    6118,   6124,   6129,   6135,   6140,   6146,   6151,   6157, 
    6162,   6168,   6173,   6179,   6185,   6190,   6196,   6201, 
    6207,   6213,   6218,   6224,   6229,   6235,   6241,   6246, 
    6252,   6258,   6263,   6269,   6275,   6280,   6286,   6292, 
    6297,   6303,   6309,   6314,   6320,   6326,   6331,   6337, 
    6343,   6349,   6354,   6360,   6366,   6372,   6377,   6383, 
    6389,   6395,   6400,   6406,   6412,   6418,   6424,   6429, 
    6435,   6441,   6447,   6453,   6458,   6464,   6470,   6476, 
    6482,   6488,   6494,   6499,   6505,   6511,   6517,   6523, 
    6529,   6535,   6541,   6546,   6552,   6558,   6564,   6570, 
    6576,   6582,   6588,   6594,   6600,   6606,   6612,   6618, 
    6624,   6630,   6636,   6642,   6648,   6654,   6660,   6666, 
    6672,   6678,   6684,   6690,   6696,   6702,   6708,   6714, 
    6720,   6726,   6732,   6738,   6744,   6750,   6757,   6763, 
    6769,   6775,   6781,   6787,   6793,   6799,   6806,   6812, 
    6818,   6824,   6830,   6836,   6843,   6849,   6855,   6861, 
    6867,   6873,   6880,   6886,   6892,   6898,   6905,   6911, 
    6917,   6923,   6930,   6936,   6942,   6948,   6955,   6961, 
    6967,   6973,   6980,   6986,   6992,   6999,   7005,   7011, 
    7018,   7024,   7030,   7037,   7043,   7049,   7056,   7062, 
    7069,   7075,   7081,   7088,   7094,   7100,   7107,   7113, 
    7120,   7126,   7133,   7139,   7145,   7152,   7158,   7165, 
    7171,   7178,   7184,   7191,   7197,   7204,   7210,   7217, 
    7223,   7230,   7236,   7243,   7249,   7256,   7263,   7269, 
    7276,   7282,   7289,   7295,   7302,   7309,   7315,   7322, 
    7328,   7335,   7342,   7348,   7355,   7362,   7368,   7375, 
    7381,   7388,   7395,   7401,   7408,   7415,   7422,   7428, 
    7435,   7442,   7448,   7455,   7462,   7469,   7475,   7482, 
    7489,   7496,   7502,   7509,   7516,   7523,   7530,   7536, 
    7543,   7550,   7557,   7564,   7570,   7577,   7584,   7591, 
    7598,   7605,   7612,   7618,   7625,   7632,   7639,   7646, 
    7653,   7660,   7667,   7674,   7681,   7687,   7694,   7701, 
    7708,   7715,   7722,   7729,   7736,   7743,   7750,   7757, 
    7764,   7771,   7778,   7785,   7792,   7799,   7806,   7813, 
    7820,   7827,   7835,   7842,   7849,   7856,   7863,   7870, 
    7877,   7884,   7891,   7898,   7906,   7913,   7920,   7927, 
    7934,   7941,   7949,   7956,   7963,   7970,   7977,   7984, 
    7992,   7999,   8006,   8013,   8021,   8028,   8035,   8042, 
    8050,   8057,   8064,   8071,   8079,   8086,   8093,   8101, 
    8108,   8115,   8123,   8130,   8137,   8145,   8152,   8159, 
    8167,   8174,   8181,   8189,   8196,   8204,   8211,   8218, 
    8226,   8233,   8241,   8248,   8256,   8263,   8271,   8278, 
    8285,   8293,   8300,   8308,   8315,   8323,   8330,   8338, 
    8346,   8353,   8361,   8368,   8376,   8383,   8391,   8398, 
    8406,   8414,   8421,   8429,   8436,   8444,   8452,   8459, 
    8467,   8475,   8482,   8490,   8498,   8505,   8513,   8521, 
    8528,   8536,   8544,   8551,   8559,   8567,   8575,   8582, 
    8590,   8598,   8606,   8613,   8621,   8629,   8637,   8645, 
    8652,   8660,   8668,   8676,   8684,   8691,   8699,   8707, 
    8715,   8723,   8731,   8739,   8747,   8754,   8762,   8770, 
};

const prog_uint8_t sine[] PROGMEM = {
       1,      1,      1,      1,      2,      2,      2,      3, 
       4,      4,      5,      5,      7,      8,      9,      9, 
      11,     12,     13,     14,     16,     18,     19,     21, 
      23,     24,     26,     28,     30,     32,     34,     36, 
      38,     40,     43,     45,     48,     50,     52,     54, 
      58,     60,     62,     66,     68,     71,     74,     77, 
      80,     82,     86,     88,     91,     94,     97,    100, 
     103,    106,    109,    113,    116,    118,    122,    125, 
     128,    131,    134,    137,    141,    144,    147,    149, 
     152,    156,    158,    162,    165,    167,    170,    174, 
     177,    179,    182,    185,    187,    190,    193,    196, 
     198,    201,    204,    206,    209,    211,    213,    215, 
     218,    220,    222,    225,    226,    228,    230,    232, 
     234,    235,    237,    239,    240,    241,    243,    244, 
     245,    247,    247,    249,    250,    251,    251,    252, 
     252,    254,    253,    254,    255,    254,    255,    255, 
     255,    255,    254,    255,    255,    254,    254,    253, 
     252,    252,    251,    250,    250,    248,    247,    246, 
     245,    244,    243,    241,    239,    238,    237,    235, 
     234,    232,    230,    228,    226,    224,    222,    220, 
     217,    216,    214,    210,    208,    206,    204,    201, 
     199,    196,    193,    191,    188,    185,    182,    179, 
     177,    174,    171,    168,    164,    162,    159,    156, 
     153,    149,    147,    143,    141,    138,    135,    131, 
     128,    125,    122,    118,    115,    112,    109,    106, 
     103,    100,     97,     94,     91,     88,     85,     83, 
      80,     77,     74,     71,     68,     66,     62,     60, 
      58,     55,     52,     50,     47,     45,     43,     40, 
      38,     36,     34,     32,     30,     28,     26,     24, 
      22,     21,     19,     18,     16,     15,     14,     12, 
      11,     10,      9,      7,      6,      6,      4,      4, 
       3,      3,      3,      2,      2,      2,      1,      1, 
       1, 
};

static inline uint8_t InterpolateSample(
    const prog_uint8_t* table,
    uint16_t phase) {
  uint8_t result;
  asm(
    "movw r30, %A1"           "\n\t"  // copy base address to r30:r31
    "add r30, %B2"            "\n\t"  // increment table address by phaseH
    "adc r31, r1"             "\n\t"  // just carry
    "lpm %0, z+"              "\n\t"  // load sample[n]
    "lpm r1, z+"              "\n\t"  // load sample[n+1]
    "mul %A2, r1"             "\n\t"  // multiply second sample by phaseL
    "movw r30, r0"            "\n\t"  // result to accumulator
    "com %A2"                 "\n\t"  // 255 - phaseL -> phaseL
    "mul %A2, %0"             "\n\t"  // multiply first sample by phaseL
    "com %A2"                 "\n\t"  // 255 - phaseL -> phaseL
    "add r30, r0"             "\n\t"  // accumulate L
    "adc r31, r1"             "\n\t"  // accumulate H
    "eor r1, r1"              "\n\t"  // reset r1 after multiplication
    "mov %0, r31"             "\n\t"  // use sum H as output
    : "=r" (result)
    : "a" (table), "a" (phase)
    : "r30", "r31"
  );
  return result;
}

static const uint16_t kPitchTableStart = 96 * 128;
static const uint16_t kOctave = 12 * 128;

volatile uint16_t phase_increment = 0;
volatile uint16_t phase = 0;
volatile uint8_t waveform = 0;
volatile uint8_t amplitude = 255;

PwmOutput<3> audio_out;
PwmOutput<11> audio_out_quadrature;

uint8_t Render(uint16_t phase) {
  switch (waveform) {
    case 0:
      return phase >> 8;

    case 1:
      return phase > 32767 ? 255 : 0;
      
    case 2:
      return (phase & 0X8000) ?
          phase >> 7 :
          ~static_cast<uint8_t>(phase >> 7);
      
    case 3:
      return InterpolateSample(sine, phase);
  }
}

TIMER_2_TICK {
  phase += phase_increment;
  uint8_t sample = 0;
  audio_out.Write(128 + (amplitude * (Render(phase) - 128) >> 8));
  audio_out_quadrature.Write(Render(phase + 16384));
}

int main(void) {
  InitAtmega(false);
  Debug::Init();
  waveform_pot.Init();
  amplitude_pot.Init();
  frequency_pot.Init();
  audio_out.Init();
  audio_out_quadrature.Init();
  Timer<2>::set_prescaler(1);
  Timer<2>::set_mode(TIMER_PWM_PHASE_CORRECT);
  Timer<2>::Start();
  
  while (1) {
    waveform = waveform_pot.Read() >> 8;
    int16_t pitch = (frequency_pot.Read() << 3) + kOctave * 2;
    int16_t ref_pitch = pitch - kPitchTableStart;
    uint8_t num_shifts = 0;
    amplitude = amplitude_pot.Read() >> 2;
    while (ref_pitch < 0) {
      ref_pitch += kOctave;
      ++num_shifts;
    }
    uint16_t increment = SimpleResourcesManager::Lookup<uint16_t, uint16_t>(
        lut_res_oscillator_increments, ref_pitch >> 1);
    increment >>= num_shifts;
    phase_increment = increment;
  }
}
